COMMENT ⊗   VALID 00010 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	ENTRY
C00005 00003	!	cmonproc
C00014 00004	! arm interactions:  read_pos,readarm,frasg,arm_check
C00016 00005	! arm interactions:  fconstructproc
C00020 00006	!	arm motions: movepcode,alongproc,axmovproc, pbyproc,ptoproc
C00032 00007	!	drivecode,opclcode,jtmove,driveproc
C00035 00008	!	centerproc,stopproc,retryproc
C00037 00009	!	opening, opclproc,closeproc
C00039 00010	!	onproc
C00044 ENDMK
C⊗;
ENTRY;
BEGIN "PPROC2"
DEFINE $$PRGID=TRUE;	

DEFINE $PPROC2=TRUE;
DEFINE $ALTER_EGO=TRUE;

REQUIRE "HEADER.SAI" SOURCE_FILE;

SIMPLE INTEGER PROCEDURE UPLEVEL(INTEGER OFFSET);
BEGIN	! eliminate this when moving to PARSE.SAI ;
	INTEGER I;
	I ← (OFFSET +1) LSH -8	; ! this gives the level ;
	I ← (I+1) LSH 8		; ! this gives the next level ;
	RETURN(I);
END;


RECORD_CLASS CLAUSE(RPTR(EXPR$)HEADER,HEAD,TAIL;
		INTEGER TYPE,VALUE;
		BOOLEAN WITH;REAL FVALUE);


DEFINE NEITHER_TYPE=0,
	EQUALITY_TYPE=1,
	RELATIONAL_TYPE=2;

DEFINE	FORCE_COND=3;	! for forces and torques;
DEFINE	TORQUE_COND=4;
DEFINE	DURATION_COND=5,
	APPROACH_COND=6,
	DEPARTURE_COND=7,
	SPEED_FACTOR_COND=8,
	FORCE_FRAME_COND=9,
	NULLING_COND=10,
	NO_NULLING_COND=11,
	STIFFNESS_COND=12,
	DRIVER_TURNS_COND=13,
	RTMOVE_COND=14,
	WOBBLE_COND=15,
	STOP_WAIT_TIME_COND=16,
	ANGULAR_VELOCITY_COND=17,
	FAILURE_COND=18,
	EXPRESSION_COND=19,
	EVENT_COND=20,
	SETBASE_COND=21;
!	cmonproc;
RECURSIVE PROCEDURE FORCECMON(RPTR(CLAUSE)CL;INTEGER BITOFFSET; BOOLEAN ABSOLUTE(FALSE));
	BEGIN
	INTEGER V; BOOLEAN GE; RPTR(EXPR$)EXP,ACTION;
	INTEGER I,IPC;
	INTEGER BITS,DEVBITS,TMPOFF;
	RPTR(SYMBOL)C;
	DEVBITS←BITOFFSET LAND '17;
	WORD_READ("("); GTOKEN;
	IF EQU(TOKEN,"XHAT") THEN BITS←BITOFFSET
		ELSE IF EQU(TOKEN,"YHAT") THEN BITS←BITOFFSET+'1000
		ELSE IF EQU(TOKEN,"ZHAT") THEN BITS←BITOFFSET+'2000
		ELSE ERROR("FORCECM: only principal directions allowed");
	WORD_READ(")");
	IF ABSOLUTE THEN BEGIN WORD_READ("|"); BITS←BITS + '20000; END;
	GTOKEN;
	IF TOKEN="≥" OR TOKEN =">" THEN BITS←BITS+'100000
		ELSE IF TOKEN="≤" OR TOKEN="<" THEN BITS←BITS
		ELSE ERROR("FORCECM: need ≥ or < here");
	EXP←$$GTANYEXP("FORCECM",#SC);
	GTOKEN;
	IF EQU(TOKEN,"IN") THEN
		BEGIN
		GTOKEN;
		IF EQU(TOKEN,"HAND") THEN BITS←BITS
			ELSE IF EQU(TOKEN,"STATION") THEN BEGIN BITS←BITS+'400; DEVBITS←DEVBITS+'400; END
			ELSE ERROR("FORCECM: can only specify in HAND or STATION");
		WORD_READ("DO");
		END
	ELSE	BEGIN IF NOT EQU(TOKEN,"DO") THEN ERROR("FORCECM: Need DO here");
		BITS←BITS+'400; DEVBITS←DEVBITS+'400; ! default is station;
		END;
	TMPOFF←$TMPOFF;	$TMPOFF←UPLEVEL($TMPOFF);
	PARSE;
	ACTION←$$PCODE;
	$TMPOFF←TMPOFF;
	$FFRCPCODE(CLAUSE:HEADER[CL],CLAUSE:HEAD[CL],CLAUSE:TAIL[CL],
		EXP,ACTION,BITS,DEVBITS,$TMPOFF);
	$TMPOFF←$TMPOFF+1;
	GTOKEN(FALSE);
	END;

RECURSIVE PROCEDURE DURCMON(RPTR(CLAUSE)CL);
	BEGIN
	INTEGER TMPOFF; RPTR(EXPR$)EXP,ACTION;
	GTOKEN;
	IF TOKEN≠">" AND TOKEN≠"≥" THEN ERROR("DURATION CMON: Need > or ≥ here");
	EXP←$$GTANYEXP("DURATION CMON",#SC);
	WORD_READ("DO");
	TMPOFF←$TMPOFF; $TMPOFF←UPLEVEL($TMPOFF);
	PARSE;
	ACTION←$$PCODE;
	$TMPOFF←TMPOFF;
	$DURCPCODE(CLAUSE:HEADER[CL],CLAUSE:HEAD[CL],CLAUSE:TAIL[CL],EXP,ACTION,$TMPOFF);
	$TMPOFF←$TMPOFF+1;
	GTOKEN(FALSE);
	END;

RECURSIVE PROCEDURE EXPCMON(RPTR(CLAUSE)CL);
	BEGIN
	RPTR(EXPR$)EXP,ACTION; INTEGER TMPOFF;
	STOKEN←TRUE;
	EXP←$$GTANYEXP("EXPRESSION CMON",#SC);
	WORD_READ("DO");
	TMPOFF←$TMPOFF; $TMPOFF←UPLEVEL($TMPOFF);
	PARSE;
	ACTION←$$PCODE;
	$TMPOFF←TMPOFF;
	$EXPCPCODE(CLAUSE:HEADER[CL],CLAUSE:HEAD[CL],CLAUSE:TAIL[CL],EXP,ACTION,$TMPOFF);
	$TMPOFF←$TMPOFF+1;
	GTOKEN(FALSE);
	END;

RECURSIVE PROCEDURE EVCMON(RPTR(CLAUSE)CL);
	BEGIN
	RPTR(EXPR$)EXP,ACTION; INTEGER TMPOFF; RPTR(SYMBOL)SYM;
	STOKEN←TRUE;
	EXP←$$GTIDREF(#EV,SYM,"EVENT CMON");
	WORD_READ("DO");
	TMPOFF←$TMPOFF;$TMPOFF←UPLEVEL($TMPOFF);
	PARSE;
	ACTION←$$PCODE;
	$TMPOFF←TMPOFF;
	$EVCPCODE(CLAUSE:HEADER[CL],CLAUSE:HEAD[CL],CLAUSE:TAIL[CL],EXP,ACTION,$TMPOFF);
	$TMPOFF←$TMPOFF+1;
	GTOKEN(FALSE);
	END;

RECURSIVE PROCEDURE CMONPROC(RPTR(CLAUSE)CL;INTEGER BITS);
	BEGIN
	INTEGER NBITS; BOOLEAN SAVERRORCMON;
	$COMPILE←$COMPILE+1;
	GTOKEN;
	SAVERRORCMON←$ERRCMON; $ERRCMON←FALSE; $ERRLEVEL←$LEVEL;
	IF TOKEN="|" THEN
		BEGIN
		GTOKEN;
		IF EQU(TOKEN,"FORCE") THEN FORCECMON(CL,BITS,TRUE)
		  ELSE IF EQU(TOKEN,"TORQUE") THEN FORCECMON(CL,BITS+'3000,TRUE);
		END
	ELSE
	IF EQU(TOKEN,"FORCE") THEN FORCECMON(CL,BITS)
	  ELSE IF EQU(TOKEN,"TORQUE") THEN FORCECMON(CL,BITS+'3000)
	  ELSE IF EQU(TOKEN,"DURATION") THEN DURCMON(CL)
	  ELSE IF EQU(TOKEN,"ERROR") THEN
		BEGIN
		$ERRCMON←TRUE;
		CLAUSE:WITH[CL]←TRUE;	! actually a WITH ;
		WORD_READ("=");
		CLAUSE:FVALUE[CL]←$GTREAL("ERROR condition monitor");
		CLAUSE:TYPE[CL]←FAILURE_COND;
		WORD_READ("DO");
		CLAUSE:TAIL[CL]←PARSE;
		GTOKEN(FALSE);
		END
	  ELSE IF (#TOKEN=ID_TYPE) AND (SYMBOL:TYPE[TOKENPTR]=#EV) THEN EVCMON(CL)
	  ELSE	EXPCMON(CL);
	$ERRCMON←SAVERRORCMON; $ERRLEVEL←$LEVEL;
	$COMPILE←$COMPILE-1;
	END;

RECURSIVE PROCEDURE WITHPROC(RPTR(CLAUSE)CL);
	BEGIN
	$COMPILE←$COMPILE+1;
	CLAUSE:WITH[CL]←TRUE;
	GTOKEN;
	IF EQU(TOKEN,"WRIST") THEN
	    BEGIN BOOLEAN NOBASE;
	    GTOKEN; IF EQU(TOKEN,"NOT") THEN NOBASE←TRUE ELSE BEGIN NOBASE←FALSE;
				GTOKEN; END;
	    IF EQU(TOKEN,"ZEROED") THEN
		IF ¬NOBASE THEN CLAUSE:HEAD[CL]←$SETBASEPCODE;
	    CLAUSE:TYPE[CL]←SETBASE_COND;
	    END
	ELSE IF EQU(TOKEN,"STIFFNESS") THEN
	    BEGIN
	    WORD_READ("=");
	    SETSTIFFPROC;
	    CLAUSE:HEAD[CL]←$$PCODE;
	    CLAUSE:TYPE[CL]←STIFFNESS_COND;
	    END
	ELSE IF EQU(TOKEN,"WOBBLE") THEN
	    BEGIN
	    WORD_READ("=");
	    CLAUSE:FVALUE[CL]←$GTREAL("WOBBLE command");
	    CLAUSE:TYPE[CL]←WOBBLE_COND;
	    END
	ELSE IF EQU(TOKEN,"DURATION") THEN
	    BEGIN
	    WORD_READ("=");
	    CLAUSE:FVALUE[CL]←$GTREAL("DURATION command");
	    CLAUSE:TYPE[CL]←DURATION_COND;
	    END
	ELSE IF EQU(TOKEN,"FORCE") THEN
		ERROR("WITH: cannot currently handle "&TOKEN)
	ELSE IF EQU(TOKEN,"NULLING") THEN
	    CLAUSE:TYPE[CL]←NULLING_COND
	ELSE IF EQU(TOKEN,"NO_NULLING") THEN
	    CLAUSE:TYPE[CL]←NO_NULLING_COND
	ELSE IF EQU(TOKEN,"SPEED_FACTOR") THEN
	    BEGIN
	    WORD_READ("=");
	    CLAUSE:FVALUE[CL]←$GTREAL("SPEED_FACTOR command");
	    CLAUSE:TYPE[CL]←SPEED_FACTOR_COND;
	    END
	ELSE IF EQU(TOKEN,"ARRIVAL") THEN
	    BEGIN
	    WORD_READ("=");
	    CLAUSE:HEAD[CL]←$$GTEXPR;
	    CLAUSE:TYPE[CL]←APPROACH_COND;
	    END
	ELSE IF EQU(TOKEN,"DEPARTURE") THEN
	    BEGIN
	    WORD_READ("=");
	    CLAUSE:HEAD[CL]←$$GTEXPR;
	    CLAUSE:TYPE[CL]←DEPARTURE_COND;
	    END
	ELSE	ERROR("WITH: cannot currently handle "&TOKEN);
	GTOKEN(FALSE);
	$COMPILE←$COMPILE-1;
	END;
! arm interactions:  read_pos,readarm,frasg,arm_check;
IFC FALSE THENC
	! assigns the value of pos(pointer or arm) to the frame fra. If direct
	  is indicated uses it to set the rotation part;

	! returns the pointer to the input device pos (arm or pointer);

RPTR (FRAME) PROCEDURE INPT_DEV(REFERENCE STRING POS);
	BEGIN
	RPTR(FRAME) FROM;
	IF EQU(POS,"BARM")
	   THEN RETURN(F_BARM)
	   ELSE IF EQU(POS,"YARM")
		   THEN RETURN(F_YARM)
		   ELSE BEGIN
			FROM←BELONGS(POS,#FR);
			WHILE FROM≠F_BARM AND FROM≠F_YARM
			   DO	BEGIN
			        PRINT("reading on arm required");
				POS←RECOVER(POS);
				FROM←BELONGS (POS,#FR);
				END;
			RETURN(FROM);
			END;
	END;

	! reads the position of the arm from, or of the arm with pointer;

PROCEDURE READ_DEV(RPTR(FRAME) FROM);
	print("dummy call to get value of the frame");

	! reads the position of the device pos (arm or pointer);

PROCEDURE INPT(REFERENCE STRING POS);
	BEGIN
	RPTR(FRAME)FROM;
	FROM←INPT_DEV(POS);
	READ_DEV(FROM);
	END;


ENDC
! arm interactions:  fconstructproc;

	! reads an axis name and returns its number:
	  xhat=0,yhat=1,zhat=2;

IFC FALSE THENC
INTEGER PROCEDURE INPT_AXIS(REFERENCE STRING AXIS);
WHILE TRUE DO
	BEGIN
	AXIS←RECOVER(AXIS);
	IF EQU(AXIS[2 TO ∞],"HAT") THEN RETURN(AXIS - "X")
		   ELSE PRINT("--→ XHAT or YHAT or ZHAT required ←--",
				CRLF,"Try again ");
	END;
	
RPTR(TRANS) ARRAY T_CSTR[1:3]; 
		! used by CONSTRUCT instruction;

	! performs a construct instruction, without arguments;

PROCEDURE FCONSTRUCTPROC;
	BEGIN
	RPTR(FRAME) ELF;RPTR(TRANS)XFE;INTEGER I;
	RPTR(FRAME) FROM;STRING POS,ANSWER,FIRST;
	RPTR(VECTOR) V1,V2,V3;
	PRELOAD_WITH 
	    	"move arm to the origin of the frame"&CRLF,
		"move arm to the axis ",
		"move arm to the plane ";
		OWN STRING ARRAY INFORM[1:3];
	STRING AXIS;INTEGER F_AXIS,S_AXIS;

	$ALLOW←$ALLOW+1;
	GTOKEN;
	IF #TOKEN≠UNDECLARED_TYPE THEN ERROR("Need undeclared token for FCONSTRUCT")
		ELSE FIRST←TOKEN;

	AXIS←NULL;
	IF F_POINTER=NULL_RECORD
	   THEN PRINT("pointer is not defined cannot be used",CRLF)
	   ELSE POS←"POINTER";
	PRINT("three positions are required",CRLF);
	FOR I←1 STEP 1 UNTIL 3 DO
		BEGIN
	! determination of the input device required;
	   	PRINT("position ",I," read on ");
		POS←RECOVER(POS);
		FROM←INPT_DEV(POS);			! checks the input device;
	! determination of the positions for reading;
		PRINT(INFORM[I]);
		IF I=2
		   THEN F_AXIS←INPT_AXIS(AXIS)
		ELSE IF I=3
		   THEN BEGIN
			PRINT(AXIS," - ");
			AXIS←NULL;
			S_AXIS←INPT_AXIS(AXIS);
			IF S_AXIS=F_AXIS THEN ERROR("instruction not executed");
			END;
	! reading of the arm position;
		PRINT("type <cr> when the arm is at the desired position");
		ANSWER←INCHRW;
		IF ANSWER=CR 
		   THEN ANSWER←INCHRW
		   ELSE	ERROR("instruction not executed");
	 	READ_DEV(FROM);				! raads the appropriate arm pos.;
		T_CSTR[I]←ABSLOC(FROM);
		END;

	! extraction of translation part;
	V1←TPOS(T_CSTR[1]);
	V2←TPOS(T_CSTR[2]);
	V3←TPOS(T_CSTR[3]);
	
	XFE←VVVTR(V1,V2,V3,F_AXIS,S_AXIS);
	ELF←FR_INSERT(FIRST);			! inserts the new frame;
	ABSSET(ELF,XFE);			! sets the new value;
	$ALLOW←$ALLOW-1;
	IFC #DISPL THENC UPDATE;ENDC	
	END;
ENDC
!	arm motions: movepcode,alongproc,axmovproc, pbyproc,ptoproc
	moveproc, parkingproc;

RECURSIVE RPTR(EXPR$)PROCEDURE FULLMOVE(RPTR(CLAUSE)ARRAY CLAUSES;
	INTEGER #CLAUSES; RPTR(EXPR$)MOVEDEC,DESTCOMP,MOVECODE,MOVEKIL);
BEGIN RPTR(RSTACK)HR,H,T;
	RPTR(EXPR$)HHR,HH,TT;
	RPTR(CLAUSE)FAILURE_CLAUSE;
	INTEGER I,#NEWVAR;
	HR←NEW_RSTACK(#CLAUSES);
	H←NEW_RSTACK(#CLAUSES);
	T←NEW_RSTACK(#CLAUSES);
	#NEWVAR←0;
	FOR I←1 STEP 1 UNTIL #CLAUSES DO
		IF CLAUSE:WITH[CLAUSES[I]] THEN
		CASE CLAUSE:TYPE[CLAUSES[I]] OF
		BEGIN
		[SETBASE_COND]
		[STIFFNESS_COND] RPUSH(HR,CLAUSE:HEAD[CLAUSES[I]]);
		[NO_NULLING_COND]
			EXPR$:BODY[MOVECODE][5]←EXPR$:BODY[MOVECODE][5] LAND 1;
		[NULLING_COND]
			EXPR$:BODY[MOVECODE][5]←EXPR$:BODY[MOVECODE][5];
		[DURATION_COND]
			EXPR$:BODY[MOVECODE][7]←CLAUSE:FVALUE[CLAUSES[I]]*1000;
		[SPEED_FACTOR_COND]
			EXPR$:BODY[MOVECODE][7]←-CLAUSE:FVALUE[CLAUSES[I]]*1000;
		[FAILURE_COND]
			BEGIN
			INTEGER J;
			J←EXPR$:#BODY[MOVECODE];
			EXPR$:BODY[MOVECODE][J-2]←CLAUSE:FVALUE[CLAUSES[I]];
			FAILURE_CLAUSE←CLAUSES[I];
			EXPR$:BODY[MOVECODE][J-1]←
				5+EXPR$:#BODY[CLAUSE:TAIL[FAILURE_CLAUSE]];
			END;
		ELSE 
		END
		ELSE
		BEGIN RPUSH(HR,CLAUSE:HEADER[CLAUSES[I]]);
		      RPUSH(H,CLAUSE:HEAD[CLAUSES[I]]);
		      RPUSH(T,CLAUSE:TAIL[CLAUSES[I]]);
		      #NEWVAR←#NEWVAR+1;
		END;
	IF RSIZE(H) THEN
		BEGIN
		RTRIM(H);
		HH←$APPEND($AAPPEND(RSTACK:STACK[H]),MOVECODE);
		END
	ELSE HH←MOVECODE;
	EXPR$:BODY[HH][I←EXPR$:#BODY[HH]] ←5-I;	! retry addr;
	IF FAILURE_CLAUSE THEN HH←$APPEND(HH,CLAUSE:TAIL[FAILURE_CLAUSE]);
	HH←$APPEND($PUSHPCPCODE,HH);
	IF RSIZE(T) THEN
		BEGIN
		RTRIM(T);
		TT←$APPEND($AAPPEND(RSTACK:STACK[T]),$KVARPCODE(#NEWVAR));
		END
	ELSE TT←$KVARPCODE(#NEWVAR);
	IF RSIZE(HR) THEN
		BEGIN
		RTRIM(HR);
		HHR←$APPEND($AAPPEND(RSTACK:STACK[HR]),HH);
		END
	ELSE HHR←HH;
	BEGIN
	RPTR(EXPR$)ARRAY TMP[1:6];
	TMP[1]←MOVEDEC;
	TMP[2]←DESTCOMP;
	TMP[3]←HHR;
	TMP[4]←TT;
	TMP[5]←MOVEKIL;
	TMP[6]←$MDONEPCODE;
	RETURN($AAPPEND(TMP));
	END;
END;

	! returns the pointer to the arm affixed to obj;
RPTR(FRAME) PROCEDURE ARM_CHECK(RPTR(FRAME) OBJ);
	BEGIN
	RPTR(FRAME) TEMP;
	TEMP←OBJ;
	WHILE TEMP≠F_WRLD DO
		IF EQU(FRAME:PNAME[TEMP],"BARM") THEN RETURN(TEMP)
 		   ELSE IF EQU(FRAME:PNAME[TEMP],"YARM") THEN ERROR("YARM cannot be moved")
			ELSE TEMP←FRAME:DAD[TEMP];
	ERROR(FRAME:PNAME[OBJ]," cannot be moved");
	END;

	! saves the first part of the instruction for move commands;
PROCEDURE OLDSAV(STRING CMD,OBJ);
	BEGIN
	OLDCMD←CMD;
	OLDOBJ←OBJ;
	END;

PROCEDURE MOVEPCODE(RPTR(FRAME) MFRAME;
		 RPTR(EXPR$) ARRAY FDESTS; INTEGER NFDEST;
		REFERENCE RPTR(EXPR$)MOVEDEC,DESTCOMP,MOVECODE,MOVEKIL);
	BEGIN
	RPTR(SYMBOL) S1,S2; RPTR(FRAME)F1; INTEGER NFDEST0;
	S1←CHECK(FRAME:PNAME[MFRAME],#FR);
	S2←CHECK(FRAME:PNAME[F1←ARM_CHECK(MFRAME)],#FR);
	$TTROFF←$TMPOFF;
	NFDEST0←NFDEST+1;
	$TMPOFF←$TMPOFF+NFDEST0;
	$MOVEPCODE(S1,S2,FDESTS,NFDEST,DESTCOMP,MOVECODE);
	MOVEDEC←$SMPDCLPCODE(#TR,NFDEST0);
	MOVEKIL←$KVARPCODE(NFDEST0);
	END;


INTERNAL PROCEDURE ALONGPROC(STRING AXIS,FRA1);
	BEGIN
	INTEGER I,INDEX;
	RPTR(expr$)SCAL;RPTR(SYMBOL)SYMPTR;RPTR(FRAME)FRAM1;
	INTEGER ARRAY BUFF1[1:3],BUFF3[1:5];
	RPTR(EXPR$)ARRAY PTR[1:3],DEST[1:1];
	SCAL←$$GTANYEXP("distance to be moved along axis",#SC);
	SYMPTR←CHECK(AXIS[1 TO 1]&"HAT",#VT);
	OLDSAV("MOVE"&AXIS[1 TO 1],FRA1);  ! saves for default instructions;
	FRAM1←BELONGS(FRA1,#FR);
	INDEX←0;
 	FOR I←XAGTVAL, SYMBOL:INDEX[SYMPTR],SYMBOL:OFFSET[SYMPTR],
		XSVMUL, XTVADD  DO BUFF3[INDEX←INDEX+1]←I;
	SYMPTR←CHECK(FRA1,#FR);
	INDEX←0;
	IF SYMBOL:INDEX[SYMPTR]>0 THEN
	    FOR I←XAGTVAL, SYMBOL:INDEX[SYMPTR],SYMBOL:OFFSET[SYMPTR]
			DO BUFF1[INDEX←INDEX+1]←I
	ELSE FOR I←XGTVAL, SYMBOL:OFFSET[SYMPTR],XNOOP
			DO BUFF1[INDEX←INDEX+1]←I;
	PTR[1]←αEXPR$(BUFF1,0);
	PTR[2]←SCAL;
	PTR[3]←αEXPR$(BUFF3,0);
	DEST[1]←$AAPPEND(PTR);
		BEGIN RPTR(EXPR$)ARRAY M[1:4];
		MOVEPCODE(FRAM1,DEST,1,M[1],M[2],M[3],M[4]);
		$$PCODE←$AAPPEND(M);
		END;
	$DISPLAYLIST[#FR]←NULL;
	END;

	! moves the frame along one axis by a scalar;

INTERNAL PROCEDURE AXMOVPROC;
	BEGIN
	STRING FRA1,AXIS; 
	AXIS←TOKEN[5 TO 5];		
	FRA1←MVFR_READ;	
	WORD_READ("BY");
	ALONGPROC(AXIS,FRA1);
	$DISPLAYLIST[#FR]←NULL;
	END;



	! reads/exec TO <fr>+<vt>{wrt <fr>} or BY <vector>{wrt <fr>};

PROCEDURE PPBYPROC(REFERENCE RPTR(EXPR$)D,C,M,K);
	BEGIN
	RPTR(EXPR$)ARRAY E[1:4];
 	RPTR(FRAME) FRAM1;RPTR(EXPR$)ARRAY FDEST[1:1];
				! MOVE<fr>BY<vt> ≡ MOVE<fr>TO⊗+<vt>;
		TOKEN←OLDOBJ;
		#TOKEN←ID_TYPE;
		STOKEN←TRUE;		
		$CLINR←"+"&$CLINR;
	FDEST[1]←$$GTANYEXP("destination of MOVE",#FR);
	FRAM1←BELONGS (OLDOBJ,#FR);
	MOVEPCODE(FRAM1,FDEST,1,D,C,M,K);
	$DISPLAYLIST[#FR]←NULL;
	E[1]←D;E[2]←C;E[3]←M;E[4]←K;
        $$PCODE←$AAPPEND(E);
	END;

PROCEDURE PPTOPROC(REFERENCE RPTR(EXPR$)D,C,M,K);
	BEGIN
 	RPTR(FRAME) FRAM1; RPTR(EXPR$) ARRAY FDESTS[1:10]; INTEGER NFDEST;
	RPTR(EXPR$)ARRAY E[1:4];
	NFDEST←0;
	DO BEGIN
		FDESTS[NFDEST←NFDEST+1]←$$GTANYEXP("Destination part of MOVE",#FR);
		IF NFDEST=10 THEN ERROR("Pointy cannot currently handle more than a 9 segment move");
		GTOKEN(FALSE);
	   END UNTIL TOKEN≠",";
	STOKEN←TRUE;
	FRAM1←BELONGS (OLDOBJ,#FR);
	MOVEPCODE(FRAM1,FDESTS,NFDEST,D,C,M,K);
	$DISPLAYLIST[#FR]←NULL;
	E[1]←D;E[2]←C;E[3]←M;E[4]←K;
        $$PCODE←$AAPPEND(E);
	END;

INTERNAL PROCEDURE PBYPROC(REFERENCE RPTR(EXPR$)C,M);
	BEGIN RPTR(EXPR$) D,K;	PPBYPROC(D,C,M,K); END;

INTERNAL PROCEDURE PTOPROC(REFERENCE RPTR(EXPR$)C,M);
	BEGIN RPTR(EXPR$) D,K; PPTOPROC(D,C,M,K); END;

INTERNAL RECURSIVE PROCEDURE MOVEPROC;
	BEGIN RPTR(EXPR$)MOVEDEC,DESTCOMP,MOVECODE,MOVEKIL; STRING FR1,AXIS;
	FR1←IDF_READ; GTOKEN;
	OLDSAV("MOVE",FR1);
	IF EQU(TOKEN,"TO") THEN PPTOPROC(MOVEDEC,DESTCOMP,MOVECODE,MOVEKIL)
		ELSE IF EQU(TOKEN,"BY") THEN PPBYPROC(MOVEDEC,DESTCOMP,MOVECODE,MOVEKIL)
	        ELSE ERROR("TO or BY required");
	GTOKEN(FALSE);
	IF EQU(TOKEN,"ON") OR EQU(TOKEN,"WITH")THEN
		BEGIN "on or with"
		RPTR(CLAUSE)ARRAY CLAUSES[1:15]; INTEGER #CLAUSES;
		INTEGER BITS,TMPOFF;
		TMPOFF←$TMPOFF; #CLAUSES←0;
		IF EQU(FR1,"BARM") THEN BITS←'4 ELSE IF
			EQU(FR1,"YARM") THEN BITS←1 ELSE
			ERROR("For force sensing can only use barm or yarm in move");
		WHILE EQU(TOKEN,"ON") OR EQU(TOKEN,"WITH") DO
		    BEGIN RPTR(CLAUSE)C; C←NEW_RECORD(CLAUSE);
			IF EQU(TOKEN,"ON")
			   THEN CMONPROC(C,BITS)
			   ELSE WITHPROC(C);
			CLAUSES[#CLAUSES←#CLAUSES+1]←C;
		    END;
		$$PCODE←FULLMOVE(CLAUSES, #CLAUSES,MOVEDEC,DESTCOMP,MOVECODE,MOVEKIL);
		$TMPOFF←TMPOFF;
		END "on or with";
	STOKEN←TRUE;
	END;

INTERNAL PROCEDURE PARKINGPROC;
	BEGIN
	STRING PAR;
	GTOKEN(FALSE);
	IF FINAL THEN ASKUSER("MOVE BARM TO BPARK; {MOVE YARM TO YPARK}")
	   ELSE IF EQU(TOKEN,"BARM") THEN ASKUSER("MOVE BARM TO BPARK")
	   ELSE IF EQU(TOKEN,"YARM") THEN ASKUSER("MOVE YARM TO YPARK")
	  ELSE ERROR("can only park BARM or YARM");
	$$PCODE←PARSE;
	END;

!	drivecode,opclcode,jtmove,driveproc;

	! drives the indicated joint of the arm (what): movement is absolute 
	  if how=to, differential if how=by;

PROCEDURE DRIVECODE(STRING WHAT,HOW;INTEGER JOINT;RPTR(EXPR$)SCAL);
	$$PCODE←$DRIVEPCODE((IF EQU(WHAT,"BJT") THEN BLUE
			ELSE YELLOW),HOW,JOINT,SCAL);

	! executes close or open instruction. How determines if the movement is 
	  absolute (to) or differential (by), op indicates the operation(open/close);

INTERNAL PROCEDURE OPCLCODE(STRING OP,HAND,HOW;RPTR(EXPR$)SCAL);
	BEGIN
	IF EQU(HAND,"BHAND")
	   THEN	IF EQU(HOW,"TO") OR EQU(OP,"OPEN")
		   THEN DRIVECODE("BJT",HOW,7,SCAL) 
		   ELSE DRIVECODE("BJT",HOW,7,$APPEND(SCAL,EXPR$1(XSNEG),#SC))
	   ELSE PRINT(#NOTYET);
	$DISPLAYLIST[#SC]←NULL;
	END;

	! parses the instruction
		DRIVE BJT|YJT (#) TO|BY <scalar>;

INTERNAL PROCEDURE JTMOVE(STRING WHAT,HOW;INTEGER JOINT);
	BEGIN "J"
	RPTR(EXPR$) SCAL;
  	SCAL←$$GTANYEXP("joint movement angle",#SC);
	OLDSAV("DRIVE",CVS(JOINT)); 			! saves for default instructions;
	IF EQU(WHAT,"BJT") THEN
		DRIVECODE(WHAT,HOW,JOINT,SCAL)
	ELSE PRINT(#NOTYET);
	$DISPLAYLIST[#FR]←NULL;
	END "J";

INTERNAL PROCEDURE DRIVEPROC;
	BEGIN
	STRING HOW;
	STRING WHAT;INTEGER JOINT;
	WHAT←IDF_READ;
	IF EQU(WHAT,"BJT") OR EQU(WHAT,"YJT")
	   THEN BEGIN
	 	WORD_READ("(");				! reads "(number)";
		GTOKEN;
		JOINT←INTSCAN(TOKEN,$BRCHR);
		IF JOINT<1 OR JOINT>7
		   THEN ERROR("non existent joint: ",cvs(joint));
		WORD_READ(")");
		HOW←IDF_READ;
		IF EQU(HOW,"BY") OR EQU(HOW,"TO")
		   THEN JTMOVE(WHAT,HOW,JOINT)
		   ELSE ERROR("TO or BY required");
		END
	   ELSE ERROR("BJT or YJT required");
	$DISPLAYLIST[#FR]←NULL;
	END;

!	centerproc,stopproc,retryproc;

INTERNAL PROCEDURE CENTERPROC;
	BEGIN "PCENTER"
	STRING POS;
	POS←ARM_READ;		! if the arm is not indicated BARM is assumed;
	IF EQU(POS,"BARM")
	   THEN	$$PCODE←$CENTERPCODE(BLUE)
	   ELSE PRINT(#NOTYET);
	END "PCENTER";

INTERNAL PROCEDURE STOPPROC;
	BEGIN "STOPPROC"
	STRING POS;
	POS←ARM_READ;
	IF EQU(POS,"BARM")
		THEN $$PCODE←$STOPPCODE(BARM_MECH)
		ELSE PRINT(#NOTYET);
	END "STOPPROC";

INTERNAL PROCEDURE RETRYPROC;
	BEGIN "RETRYPROC"
	IF NOT $ERRCMON THEN ERROR("RETRY: only valid inside an ERROR condition monitor");
	IF ($ERRLEVEL≠$LEVEL) AND ($ERRLEVEL+1≠$LEVEL) THEN
		ERROR("RETRY: must be the same lexical level as the block of theerror condition");
	$$PCODE←$PRETRYPCODE;
	END "RETRYPROC";
!	opening, opclproc,closeproc;

INTERNAL PROCEDURE OPENING(STRING FIRST,WHAT,HOW);
	BEGIN
	RPTR(EXPR$)SCAL;
	SCAL←$$GTANYEXP("hand opening or closing",#SC);
	OLDSAV(FIRST,WHAT);			! saves for default instructions;
	OPCLCODE(FIRST,WHAT,HOW,SCAL);
	END;

	! parses the instructions
		OPEN <hand> TO|BY <scalar>;
	!	CLOSE <hand> TO|BY <scalar>;

INTERNAL PROCEDURE OPCLPROC(STRING FIRST);
	BEGIN
	STRING WHAT;
	WHAT←HAND_READ;
	GTOKEN;
	IF EQU(TOKEN,"TO") OR EQU(TOKEN,"BY")
	   THEN OPENING(FIRST,WHAT,TOKEN)
	   ELSE ERROR("Need a TO or BY for OPEN/CLOSE statement");
	END;

	! parses the instructions
	  CLOSE <hand> TO|BY <scalar> 	(BHAND as default);

INTERNAL PROCEDURE CLOSEPROC;
	BEGIN
	STRING HAND,HOW;
	GTOKEN;
	IF EQU(HAND←TOKEN,"BHAND") OR EQU(TOKEN,"YHAND") 
	    THEN GTOKEN
	    ELSE HAND←"BHAND";
	IF EQU(HOW←TOKEN,"BY") OR EQU(TOKEN,"TO")
	    THEN OPENING("CLOSE",HAND,HOW)
	    ELSE ERROR("CLOSE: need hand opening TO or BY");
	END;
!	onproc;
	
INTERNAL PROCEDURE ONPROC(RPTR(EXPR$)E(NULL_RECORD));
IFC FALSE THENC
	BEGIN
!	IF $COMPILE=0 THEN ERROR("ON must be inside a procedure");
	$COMPILE←$COMPILE+1;
	GTOKEN;
	IF EQU(TOKEN,"FORCE") THEN FORCECM(E,0)
	  ELSE IF EQU(TOKEN,"TORQUE") THEN FORCECM(E,'3000)
	  ELSE ERROR("ON: only FORCE or TORQUE available");
	$COMPILE←$COMPILE-1;
	END;
ENDC;

END "PPROC2";